%%%-------------------------------------------------------------------
%%% @author liuwentao
%%% @doc
%%%
%%% @end
%%% Created : 23. 6月 2021 17:51
%%%-------------------------------------------------------------------
-module(a_path).
-author("liuwentao").

%% API
-export([]).

%%%-----------------------------------
%% @Description: 寻路逻辑
%%%-----------------------------------
-define(BLOCK, 1).
-record(node, {
    x = 0
    , z = 0
    , g = 0
    , h = 0
    , f = 0
    , father = {-99999, -99999}
}).
-export([
    astar_search/5		%% A星寻路
]).


%% A星寻路
%% return : [{PosX, PosZ}] , 列表从终点到起点排列)
astar_search(_MapId, StartX, StartZ, EndX, EndZ) ->
    case is_block(EndX, EndZ) of
        %% 终点为障碍点，返回空
        true -> [];
        false ->
            StartNode = #node{
                x = StartX
                , z = StartZ
                , g = 0
                , h = 0
                , f = 0
                , father = {-99999, -99999}
            },
            OpenList = [StartNode],
            CloseList = [],
            put(end_point, {EndX, EndZ}),
            start_search(OpenList, CloseList)
    end.







%% ====================================================内部处理逻辑==================================================== %%
%% ====================================================内部处理逻辑==================================================== %%
%% ====================================================内部处理逻辑==================================================== %%
%% A星寻路核心逻辑
start_search([], _CloseList) -> []; %% 路径不存在
start_search(OpenList, CloseList) ->
    {MinNode, OpenList2} = get_min_node(OpenList),
    {EndX, EndZ} = get(end_point),
    case MinNode#node.x =:= EndX andalso MinNode#node.z =:= EndZ of
        %% 找到路径
        true -> build_path(MinNode, [], MinNode#node.f);
        false ->
            %% 查找八个方向
            {NewOpenList, CloseList2} = search_for_eight_dir(OpenList2, CloseList, MinNode, lists:seq(1, 8)),
            %% 节点放入到 close list 里面
            NewCloseList = CloseList2 ++ [MinNode],
            put({MinNode#node.x, MinNode#node.z}, MinNode),
            start_search(NewOpenList, NewCloseList)
    end.


%% 在open_list中找到最佳点,并删除
get_min_node([]) -> {#node{}, []};
get_min_node(OpenList) ->
    MinI = get_min_node_i(9999, 0, 0, OpenList),
    Node = lists:nth(MinI, OpenList),
    NewOpenList = lists:delete(Node, OpenList),
    {Node, NewOpenList}.
get_min_node_i(_MinValue, MinNth, _NowNth, []) -> MinNth;
get_min_node_i(MinValue, MinNth, NowNth, [Node | RestList]) ->
    case Node#node.f < MinValue of
        true -> get_min_node_i(Node#node.f, NowNth + 1, NowNth + 1, RestList);
        false -> get_min_node_i(MinValue, MinNth, NowNth + 1, RestList)
    end.


%% 计算路径
build_path(undefined, Paths, SumCost) -> {Paths, SumCost};
build_path(Node, Paths, SumCost) ->
    NewPaths = [{Node#node.x, Node#node.z} | Paths],
    build_path(get(Node#node.father), NewPaths, SumCost).


%% 查找八个方向
search_for_eight_dir(OpenList, CloseList, _MinNode, []) -> {OpenList, CloseList};
search_for_eight_dir(OpenList, CloseList, MinNode, [Nth | Rest]) ->
    {X, Z} =
        case Nth of
            1 -> {MinNode#node.x,     MinNode#node.z + 1};
            2 -> {MinNode#node.x,     MinNode#node.z - 1};
            3 -> {MinNode#node.x + 1, MinNode#node.z    };
            4 -> {MinNode#node.x + 1, MinNode#node.z + 1};
            5 -> {MinNode#node.x + 1, MinNode#node.z - 1};
            6 -> {MinNode#node.x - 1, MinNode#node.z    };
            7 -> {MinNode#node.x - 1, MinNode#node.z + 1};
            8 -> {MinNode#node.x - 1, MinNode#node.z - 1}
        end,
    case is_block(X, Z) of
        true -> search_for_eight_dir(OpenList, CloseList, MinNode, Rest);
        false ->
            CurNode = get_fgh(MinNode, X, Z),
            Open = node_in_list(X, Z, OpenList),
            Close = node_in_list(X, Z, CloseList),
            case Open =:= false andalso Close =:= false of
                %% 不在OPEN表和CLOSE表中
                %% 添加特定节点到 open list
                true ->
                    NewOpenList = OpenList ++ [CurNode],
                    NewCloseList = CloseList;
                false ->
                    case Open of
                        %% 在CLOSE表中
                        false ->
                            CloseNode = Close,
                            case CloseNode#node.f > CurNode#node.f of
                                true ->
                                    NewOpenList = OpenList ++ [CurNode],
                                    NewCloseList = lists:delete(CloseNode, CloseList),
                                    put({CurNode#node.x, CurNode#node.z}, CurNode);
                                false ->
                                    NewOpenList = OpenList,
                                    NewCloseList = CloseList
                            end;
                        %% 在OPEN表
                        OpenNode ->
                            case OpenNode#node.f > CurNode#node.f of
                                %% 更新OPEN表中的估价值
                                true ->
                                    NewOpenList = node_replace_list(OpenNode, CurNode, OpenList, []),
                                    NewCloseList = CloseList;
                                false ->
                                    NewOpenList = OpenList,
                                    NewCloseList = CloseList
                            end
                    end
            end,
            search_for_eight_dir(NewOpenList, NewCloseList, MinNode, Rest)
    end.


%% 获取 f ,g ,h
get_fgh(Father, X, Z) ->
    Cost =
        case Father#node.x =:= X orelse Father#node.z =:= Z of
            true -> 1;
            false -> 1.414
        end,
    G = Father#node.g + Cost,
    H = diagonal(X, Z),
    F = G + H,
    Node = #node{
        x = X
        , z = Z
        , g = G
        , h = H
        , f = F
        , father = {Father#node.x, Father#node.z}
    },
    Node.


%% 对角线估价法,先按对角线走，一直走到与终点水平或垂直平行后，再笔直的走
diagonal(X, Z) ->
    {EndX, EndZ} = get(end_point),
    Dx = abs(X - EndX),
    Dz = abs(Z - EndZ),
    MinD = min(Dx, Dz),
    H = MinD * 1.414 + Dx + Dz - 2 * MinD,
    H.


node_in_list(_X, _Z, []) -> false;
node_in_list(X, Z, [Node | RestList]) ->
    case Node#node.x =:= X andalso Node#node.z =:= Z of
        true -> Node;
        false -> node_in_list(X, Z, RestList)
    end.


%% 用NewNode把List中的OldNode替换掉
node_replace_list(_OldNode, _NewNode, [], NewList) -> lists:reverse(NewList);
node_replace_list(OldNode, NewNode, [Node | RestList], NewList) ->
    case OldNode =:= Node of
        true -> node_replace_list(OldNode, NewNode, RestList, [NewNode | NewList]);
        false -> node_replace_list(OldNode, NewNode, RestList, [Node | NewList])
    end.


%% 是否障碍点
is_block(X, Z) ->
    case map:get(X, Z) of
        ?BLOCK -> true;
        _ ->
            X < map:get_min_x() orelse X > map:get_max_x() orelse Z < map:get_min_z() orelse Z > map:get_max_z()
    end.